#include "mylib.h"
namespace mylib
{
  namespace linalg
  {
    void gaussj(MatDoub_IO &a, MatDoub_IO &b)
    {
      Int i, icol, irow, j, k, l, ll, n = a.nrows(), m = b.ncols();
      Doub big, dum, pivinv;
      VecInt indxc(n), indxr(n), ipiv(n);
      for (j = 0; j < n; j++)
        ipiv[j] = 0;
      for (i = 0; i < n; i++)
      {
        big = 0.0;
        for (j = 0; j < n; j++)
          if (ipiv[j] != 1)
            for (k = 0; k < n; k++)
            {
              if (ipiv[k] == 0)
              {
                if (abs(a[j][k]) >= big)
                {
                  big = abs(a[j][k]);
                  irow = j;
                  icol = k;
                }
              }
            }
        ++(ipiv[icol]);
        if (irow != icol)
        {
          for (l = 0; l < n; l++)
            SWAP(a[irow][l], a[icol][l]);
          for (l = 0; l < m; l++)
            SWAP(b[irow][l], b[icol][l]);
        }
        indxr[i] = irow;
        indxc[i] = icol;
        if (a[icol][icol] == 0.0)
          throw("gaussj: Singular Matrix");
        pivinv = 1.0 / a[icol][icol];
        a[icol][icol] = 1.0;
        for (l = 0; l < n; l++)
          a[icol][l] *= pivinv;
        for (l = 0; l < m; l++)
          b[icol][l] *= pivinv;
        for (ll = 0; ll < n; ll++)
          if (ll != icol)
          {
            dum = a[ll][icol];
            a[ll][icol] = 0.0;
            for (l = 0; l < n; l++)
              a[ll][l] -= a[icol][l] * dum;
            for (l = 0; l < m; l++)
              b[ll][l] -= b[icol][l] * dum;
          }
      }
      for (l = n - 1; l >= 0; l--)
      {
        if (indxr[l] != indxc[l])
          for (k = 0; k < n; k++)
            SWAP(a[k][indxr[l]], a[k][indxc[l]]);
      }
    }

    void gaussj(MatDoub_IO &a)
    {
      MatDoub b(a.nrows(), 0);
      gaussj(a, b);
    }
    void gaussj2(MatDoub_IO &a, VecDoub_IO &b)
    {
      int n, index;
      double max;
      n = b.size();
      for (int k = 0; k < n - 1; k++)
      {
        // 选主元
        max = abs(a[k][k]);
        index = k;
        for (int i = k; i < n; i++)
        {
          if (abs(max) < abs(a[i][k]))
          {
            max = a[i][k];
            index = i;
          }
        }
        // 交换最大行
        if (index != k)
        {
#ifdef DEBUG
          std::cout<<"交换第"<<k<<"与"<<index<<"行"<<std::endl;
#endif
          for (int ii = 0; ii < n; ii++)
          {
            max = a[k][ii];
            a[k][ii] = a[index][ii];
            a[index][ii] = max;
          }
          max = b[k];
          b[k] = b[index];
          b[index] = max;
        }
        if (a[k][k] == 0.0)
          std::cout << "对角元为零" << std::endl;
        //消元
        for (int ii = k + 1; ii < n; ii++)
        {
          for (int jj = k + 1; jj < n; jj++)
          {
            a[ii][jj] = a[ii][jj] - a[ii][k] / a[k][k] * a[k][jj];
          }
          b[ii] = b[ii] - a[ii][k] / a[k][k] * b[k];
        }
#ifdef DEBUG 
        a.show();
#endif
      } // k

      for (int i = 1; i < n; i++)
      {
        for (int j = 0; j < i; j++)
        {
          a[i][j] = 0.0;
        }
      }
    } //gaussj2
  }
}